#include "outline/bezier.h"
#include "glib/g4/smatrix.h"

namespace outline {

template <class R>
template <class T>
typename bezier<R>::real_type bezier<R>::best_fit_free_ends(
	std::vector< point<T> > const & points, real_vector_type const & parameters)
{
	uint n = parameters.size();
	assert (n == points.size());
	assert (n >= 4);

	typedef g4::basic_vector<accu_type> vector4;
	typedef g4::basic_smatrix<accu_type> smatrix4;

	//sums of square weights of i-th  and j-th control points (divided on 9)
	smatrix4 sum_r_r;
	sum_r_r.zero();
	//sums of products of i-th control point weight and appropriate absciss value of target point (divided on 3)
	vector4 sum_r_x;
	sum_r_x.zero();
	vector4 sum_r_y;
	sum_r_y.zero();
	accu_type sum_x_x = 0;
	accu_type sum_y_y = 0;

	for (uint i = 0; i < n; ++i) {
		accu_type t = parameters[i];
		accu_type u = 1-t;
		vector4 r(u*u*u, 3*t*u*u, 3*u*t*t, t*t*t);
		point<accu_type> p = points[i];

		sum_r_r += tsquare(r);

		sum_r_x += p.x * r;
		sum_r_y += p.y * r;

		sum_x_x += p.x * p.x;
		sum_y_y += p.y * p.y;
	}

	smatrix4 inv_sum_r_r = sum_r_r.inverse();

	vector4 sol_x = inv_sum_r_r * sum_r_x;
	vector4 sol_y = inv_sum_r_r * sum_r_y;

	for (uint j = 0; j < 4; ++j) {
		p[j].x = real_type(sol_x(j));
		p[j].y = real_type(sol_y(j));
	}

	accu_type chisq_x = sum_x_x - scalar_product(sum_r_x, sol_x);
	accu_type chisq_y = sum_y_y - scalar_product(sum_r_y, sol_y);
	accu_type chisq = chisq_x + chisq_y;

	return (chisq >= 0) ? chisq : 0;
}

template float bezier<float>::best_fit_free_ends(outline_us const & points, std::vector<float> const & parameters);
template float bezier<float>::best_fit_free_ends(outline_s const & points, std::vector<float> const & parameters);
template float bezier<float>::best_fit_free_ends(outline_i const & points, std::vector<float> const & parameters);
template float bezier<float>::best_fit_free_ends(outline_f const & points, std::vector<float> const & parameters);
template double bezier<double>::best_fit_free_ends(outline_d const & points, std::vector<double> const & parameters);

} //namespace outline
